package robot;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;


import net.sf.json.JSONObject;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;

import network.NetworkCommunication;
import log.Log;
import path.Path;
import pose.Pose;
import robot.differentialdrive.DifferentialDrive;
import robot.differentialdrive.DifferentialDriveCommand;
import robot.differentialdrive.DifferentialDriveFeedback;
import robot.differentialdrive.DifferentialDriveProperties;
import robot.lasers.LaserScan;

import config.Config;


public class Robot {

	private static final String LOG_TAG = Robot.class.getSimpleName();

	private static double ERROR_DISTANCE = 0.5;
	@SuppressWarnings("unused")
	private static double WARNING_DISTANCE = 0.3;
	private static double LOOK_AHEAD = 0.625;

	private DifferentialDrive differentialDrive;
	private LaserScan sensors;
	private Pose pose;
	private Path pathToFollow;
	private double lookAheadDistance;

	public Robot(DifferentialDrive differentialDrive, LaserScan sensors,
			Pose pose, Path pathToFollow) {
		super();
		this.differentialDrive = differentialDrive;
		this.sensors = sensors;
		this.pose = pose;
		this.pathToFollow = pathToFollow;
		lookAheadDistance = LOOK_AHEAD;
	}

	public Robot(){
		super();
		pose = new Pose();
		DifferentialDriveProperties dp = NetworkCommunication.initDifferentialDrivePropertiesFromServer();
		differentialDrive = new DifferentialDrive(new DifferentialDriveCommand(0, 0), new DifferentialDriveFeedback(0, 0, true), dp, 0);
		differentialDrive.getCommand().setProperties(dp);
		sensors = new LaserScan();
		lookAheadDistance = LOOK_AHEAD;
	}

	/**
	 * @param params a hashmap created with attributes given to main function
	 */
	public Robot(HashMap<String, String> params){
		super();
		pose = new Pose();
		DifferentialDriveProperties dp = NetworkCommunication.initDifferentialDrivePropertiesFromServer();
		differentialDrive = new DifferentialDrive(new DifferentialDriveCommand(0, 0), new DifferentialDriveFeedback(0, 0, true), dp, 0);
		differentialDrive.getCommand().setProperties(dp);
		sensors = new LaserScan();
		if(params.containsKey(Config.ARGS_LOOKAHEAD)){
			LOOK_AHEAD = Double.valueOf(params.get(Config.ARGS_LOOKAHEAD));
		}
		if(params.containsKey(Config.ARGS_ERROR_DISTANCE)){
			ERROR_DISTANCE = Double.valueOf(params.get(Config.ARGS_ERROR_DISTANCE));
		}
		if(params.containsKey(Config.ARGS_WARNING_DISTANCE)){
			WARNING_DISTANCE = Double.valueOf(params.get(Config.ARGS_WARNING_DISTANCE));
		}
	}

	public DifferentialDrive getDifferentialDrive() {
		return differentialDrive;
	}
	
	public void setDifferentialDrive(DifferentialDrive differentialDrive) {
		this.differentialDrive = differentialDrive;
	}
	
	public LaserScan getSensors() {
		return sensors;
	}
	
	public void setSensors(LaserScan sensors) {
		this.sensors = sensors;
	}
	
	public Pose getPose() {
		return pose;
	}
	public void setPose(Pose pose) {
		this.pose = pose;
	}
	public Path getPathToFollow() {
		return pathToFollow;
	}
	public void setPathToFollow(Path pathToFollow) {
		this.pathToFollow = pathToFollow;
	}

	public double getLookAheadDistance() {
		return lookAheadDistance;
	}

	/**
	 * Retrieve robot pose from server
	 */
	public void getCurrentPose(){
		JSONObject obj = NetworkCommunication.getRobotPositionFromServer();
		pose.setPoseFromJSONObject(obj);
	}

	/**
	 * Retrieve the differential drive from the server
	 */
	public void getDifferentialDriveFromNetwork(){
		JSONObject obj = NetworkCommunication.getRobotDifferentialDriveFromServer();
		differentialDrive.setDifferentialDriveFromJSONObject(obj); 
	}

	/**
	 * Calculate an estimation of time needed to go to a point considering linear speed
	 * @param distance the distance of the point to reach
	 * @return estimated time in ms
	 */
	public long estimateTimeOfArrivalLinear(double distance){
		return (long) (1000 * (1 / (Math.abs(this.differentialDrive.getCommand().getTargetLinearSpeed() / distance))));
	}

	/**
	 * Calculate an estimation of time needed to rotate to be align with a point considering angular speed
	 * @param distance the angle of the point to reach
	 * @return estimated time in ms
	 */
	public long estimateTimeOfArrivalAngular(double distance){
		return (long) (1000 * (1 / (Math.abs(this.differentialDrive.getCommand().getTargetAngularSpeed() / distance))));
	}

	public void calulateLookAheadDistance(Path p){
		int nbPointBefore = 0;
		Vector3D toCompare;
		for(int i = 0; i < p.getPath().size(); i++){
			toCompare = p.getPath().get(i).convertPositionToRobotCoordinateSystem(this);
			if(toCompare.getNorm() < 1){
				nbPointBefore++;
			}
		}
		lookAheadDistance = (p.getPath().size() - nbPointBefore)/p.getPath().size();
	}

	public void followTheCarrot(int positionGoalPoint){
		long eta;
		Vector3D goalPoint = pathToFollow.getPath().get(positionGoalPoint).convertPositionToRobotCoordinateSystem(this);
		double angle = Math.atan2(goalPoint.getY(), goalPoint.getX());
		double distance = goalPoint.getNorm();
		this.getDifferentialDrive().getCommand().setTargetAngularSpeed(this.getDifferentialDrive().getProperties().getMaxAngularSpeed());//nearestPoint.getX()/nearestPoint.getY()));
		eta = this.estimateTimeOfArrivalAngular(angle);
		NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
		waitTime(eta);
		stop();
		this.getDifferentialDrive().getCommand().setTargetLinearSpeed(this.getDifferentialDrive().getProperties().getMaxLinearSpeed());
		eta = this.estimateTimeOfArrivalLinear(distance);
		NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
		waitTime(eta);
		stop();
		deletePointsFromPath(positionGoalPoint);
	}

	/**
	 * An iteration of pure pursuit algorithm
	 * @param positionGoalPoint the index of the goal point in the path
	 */
	public void purePursuit(int positionGoalPoint){
		Vector3D goalPoint = pathToFollow.getPath().get(positionGoalPoint).convertPositionToRobotCoordinateSystem(this);
		double angle = Math.atan2(goalPoint.getY(), goalPoint.getX());
		double radius = 1/ (2 * (goalPoint.getY()/Math.pow(goalPoint.getNorm(), 2)));
		double distance = Math.abs(radius * angle);
		boolean avoided = false;
		double linearSpeed = 0;
		if(Config.AVOID_OB){
			avoided = avoidObstacles(goalPoint, radius);
		}
		
			goalPoint = pathToFollow.getPath().get(positionGoalPoint).convertPositionToRobotCoordinateSystem(this);
			if(Config.DYNAMIC_SPEED){
				linearSpeed = distance*2;
			}else{
				linearSpeed = Config.SET_SPEED;
			}
			this.getDifferentialDrive().getCommand().setTargetLinearSpeed(linearSpeed);
			this.getDifferentialDrive().getCommand().setTargetAngularSpeed(this.getDifferentialDrive().getCommand().getTargetLinearSpeed()/radius);
			NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
			waitPosition(positionGoalPoint);
			stop();
		if(Config.AVOID_OB && !avoided){
			deletePointsFromPath(positionGoalPoint);
		}else if(!Config.AVOID_OB){
			deletePointsFromPath(positionGoalPoint);
		}
	}

	/**
	 * Delete all points before the index of goal point
	 * @param positionGoal
	 */
	private void deletePointsFromPath(int positionGoal){
		for(int i = positionGoal; i >= 0; i--){
			pathToFollow.getPath().remove(0);
		}
	}

	/**
	 * Send a stop request to the server, ie. angular and linear speed are set to 0
	 */
	private void stop(){
		this.getDifferentialDrive().getCommand().setTargetAngularSpeed(0);
		this.getDifferentialDrive().getCommand().setTargetLinearSpeed(0);
		NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
	}

	

	/**
	 * @param goalPoint
	 * @return
	 */
	private boolean avoidObstacles(Vector3D goalPoint, double radius){
		double angle = Math.atan2(goalPoint.getY(), goalPoint.getX());
		long eta;
		if(angle < sensors.getLasersProperties().getStartAngle() || angle > sensors.getLasersProperties().getEndAngle()){
			Log.e(LOG_TAG, "ANGLE TOO LARGE ROTATE BEFORE !!!");
			differentialDrive.getCommand().setTargetAngularSpeed(differentialDrive.getProperties().getMaxAngularSpeed());
			eta = estimateTimeOfArrivalAngular(angle);
			NetworkCommunication.postSpeedToServer(differentialDrive.getCommand().toJSONObject());
			waitTime(eta);
			stop();
			getCurrentPose();
			angle = Math.atan2(goalPoint.getY(), goalPoint.getX());
		}
		sensors.getCurrentLasers();
		int indexAngleSensors = sensors.getIndexLaserOfAngle(angle);
		int indexAlignWithQuaternion = indexAngleSensors - ((int) (angle / sensors.getLasersProperties().getAngleIncrement()));
		double anglei;
		double distancei;
		int end = Math.max(indexAlignWithQuaternion, indexAngleSensors);
		int start = Math.min(indexAlignWithQuaternion, indexAngleSensors);
		ArrayList<Integer> laserIndex = new ArrayList<Integer>();
		ArrayList<Double> distances = new ArrayList<Double>();
		ArrayList<Double> theoreticalDistances = new ArrayList<Double>();
		for(int i = start; i < end; i++){
			anglei = sensors.getLasersProperties().getStartAngle() + i * sensors.getLasersProperties().getAngleIncrement();
			distancei = 2*radius*Math.sin(anglei/2);
			if(i >= 0 && i < sensors.getLasersEchoes().size() && sensors.getLasersEchoes().get(i) < distancei){
				Log.e(LOG_TAG, "WARNING ZONE !");
				laserIndex.add(i);
				distances.add(sensors.getLasersEchoes().get(i));
				theoreticalDistances.add(distancei);
			}
		}
		if(distances.size() != 0){
			Double min = Collections.min(distances);
			int index = laserIndex.get(distances.indexOf(min));
			double correctionAngle = sensors.getLasersProperties().getStartAngle() + index * sensors.getLasersProperties().getAngleIncrement() - Math.PI;
			Log.i(LOG_TAG, "Correction Angle : " + correctionAngle);
			differentialDrive.getCommand().setTargetAngularSpeed(differentialDrive.getProperties().getMaxAngularSpeed());
			eta = estimateTimeOfArrivalAngular(correctionAngle);
			NetworkCommunication.postSpeedToServer(differentialDrive.getCommand().toJSONObject());
			waitTime(eta);
			stop();
			double avoid = theoreticalDistances.get(distances.indexOf(min)) - min;
			differentialDrive.getCommand().setTargetLinearSpeed(differentialDrive.getProperties().getMaxLinearSpeed());
			eta = estimateTimeOfArrivalLinear(avoid);
			NetworkCommunication.postSpeedToServer(differentialDrive.getCommand().toJSONObject());
			waitTime(eta);
			stop();
			getCurrentPose();
			return true;
		}
		return false;
	}
	
	/**
	 * Waiting function considering position of the robot
	 * @param positionGoalPoint the index of goalPoint in the path list
	 */
	private void waitPosition(int positionGoalPoint){
		Vector3D goal;
		double norm;
		do{
			this.getCurrentPose();
			goal = pathToFollow.getPath().get(positionGoalPoint).convertPositionToRobotCoordinateSystem(this);
			norm = goal.getNorm();
		}while(norm > ERROR_DISTANCE);
	}

	/**
	 * Waiting function considering estimate time to go to a point
	 * @param estimateTimeOfArrival
	 */
	private void waitTime(long estimateTimeOfArrival){
		try{
			Thread.sleep(estimateTimeOfArrival);
		}catch(InterruptedException e){
			e.printStackTrace();
		}
	}
}
